

#pragma once
#include "rclcpp/rclcpp.hpp"
#include <string>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include "rclcpp_action/rclcpp_action.hpp"
#include <ipa_building_navigation/SimpleActionClientD.h>
#include <ipa_building_msgs/action/find_room_sequence_with_checkpoints.hpp>
#include <ipa_building_msgs/action/map_segmentation.hpp>
#include <ipa_building_msgs/action/room_exploration.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <nav_msgs/msg/path.hpp>
#include "nav2_msgs/srv/get_map_nav2.hpp"
#include <geometry_msgs/msg/pose.hpp>
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/utils.h"

using ClientSegmentation=ipa_building_msgs::action::MapSegmentation;
using ClientSequence=ipa_building_msgs::action::FindRoomSequenceWithCheckpoints;
using ClientExploration=ipa_building_msgs::action::RoomExploration;

class IpaRoomPlanning 
{

    public:
        struct IpaRoomPlanningParam
        {
            IpaRoomPlanningParam(){};
            std::string image_file="";
            std::vector<geometry_msgs::msg::Point32> fov_points;
            std::vector<double> map_origin;
            std::vector<double> robot_pos;
            std::vector<double> start_pos;
            geometry_msgs::msg::Point32 fov_origin;
            double map_resolution = 0.05;
            double robot_radius = 0.15;
            double coverage_radius=0.15;
            int planning_mode=1;
            bool return_format_in_meter = false;
            bool return_format_in_pixel = true;
            bool display=false;
            bool do_flip=true;
        };

        IpaRoomPlanning(rclcpp::Node::SharedPtr &nh,std::shared_ptr<tf2_ros::Buffer> tf_);
        bool prepareData();
        bool getRobotPose(std::shared_ptr<tf2_ros::Buffer> tf_,std::vector<double>& robot_pos);

        void fromPathGetMat(std::string image_path);
        void matToMap(nav_msgs::msg::OccupancyGrid &map, const cv::Mat &mat);
        void mapToMat(const nav_msgs::msg::OccupancyGrid &map, cv::Mat &mat);

        bool getRoomSegmentation(IpaRoomPlanningParam & segmentation_param);
        bool getRoomSequence(IpaRoomPlanningParam & sequence_param);
        bool getRoomExploration(IpaRoomPlanningParam & exploration_param,cv::Mat &exploration,cv::Mat & display_mat);

        bool getRoomSegmentation()  {return getRoomSegmentation(ipa_planning_param_);};
        bool getRoomSequence()      {return getRoomSequence(ipa_planning_param_);};
        bool getRoomExploration(cv::Mat &exploration,cv::Mat & display_mat)   
            {return getRoomExploration(ipa_planning_param_,exploration,display_mat);};

        void Binarization(cv::Mat &map_copy,int threshold,int min_value=0,int max_value=255);
        void Publish();
        IpaRoomPlanningParam getParem(){   return  ipa_planning_param_;};

    private:

	    rclcpp::Node::SharedPtr node_handle_;
        rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr all_path_pub;
        rclcpp::Client<nav2_msgs::srv::GetMapNav2>::SharedPtr     grid_client;
        std::shared_ptr<SimpleActionClientD<ClientSegmentation>>  room_segmentation_client;
        std::shared_ptr<SimpleActionClientD<ClientSequence>>      room_sequence_client;
        std::shared_ptr<SimpleActionClientD<ClientExploration>>   room_exploration_client;
        std::shared_ptr<tf2_ros::Buffer> tf_buffer=nullptr;
        
        sensor_msgs::msg::Image input_image_msg;
        cv::Mat origin_map_mat;
        double free_thresh;
	    double occupied_thresh;

    public:
        std::shared_ptr<ClientSegmentation::Result> result_seg=nullptr;
        std::shared_ptr<ClientSequence::Result> result_seq=nullptr;
        std::shared_ptr<ClientExploration::Result> result_exp=nullptr;
        std::vector< std::vector<geometry_msgs::msg::PoseStamped>> all_room_paths;
        cv::Mat colour_segmented_map;
        cv::Mat exploration_map;
        IpaRoomPlanningParam ipa_planning_param_;
};


